/** ###################################################################
**     Filename  : CommandSystem.c
**     Project   : parking
**     Processor : MC9S12XS128MAL
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 2013-2-23, 14:36
**     Contents  :
**         User source code
**
** ###################################################################*/

/* MODULE CommandSystem */

#include "CommandSystem.h"
#include "Communication.h"

#include <ctype.h>
#include <assert.h>
#include <stddef.h>
#include "Hmi.h"
#include "Communication.h"
#include "Cpu.h"
#include "ParkControl.h"
#include <stdio.h>
#include "Drv.h"

U8 CSpBuf[CS_BUF_LEN + 1];
U8 CSpBufPtr = 0;
U8 CSpState = CS_OK;
U8 CSpNextState = CS_OK;
U8 CSpFSCount = 0;

//CSCommand CSCmdBuf[CS_COMMAND_BUF_SIZE];
//U8 CSCmdBufWriteCount = 0;
//U8 CSCmdBufReadCount = 0;
U8 CSCommandState = CS_WAITING;
U8 CSNextCommandState = CS_WAITING;

S16 CS_atoi(U8 *p, U8 *pos){
	S16 rst = 0;
	bool neg_flag = FALSE;
	*pos = 0;
    if (p[0] == '+' || p[0] == '-'){
        neg_flag = (p[0] != '+');
        ++(*pos); 
    }
    while(isdigit(*(p + *pos))){
    	rst = rst * 10 + (*(p + *pos) - '0');	
    	++(*pos);
    }
	return neg_flag?0 - rst: rst;	
}


static void CS_flush(void){
	U8 i;
	for (i = 0; i < CS_BUF_LEN; ++i){
		CSpBuf[i] = 0;		
	}
	CSpBufPtr = 0;
	CSpFSCount = 0;	
}

void CSInit(void){
	CS_flush();
	CSpBuf[CS_BUF_LEN] = 0;
	CSpState = CS_OK;
	CSCommandState = CS_WAITING;
//	PCLoadProcedure();
	
}

static bool CS_is_valid_char(U8 c){
	return isdigit(c) || c == CS_CS || c == CS_FS || c =='-';
}

/*
static void CS_save_cmd(S16 seq, S16 cmd, S16 value){
	U8 num = CSCmdBufWriteCount % CS_COMMAND_BUF_SIZE;
	Cpu_SetIPL(6); 
	CSCmdBuf[num].seq = seq;
	CSCmdBuf[num].command = cmd;
	CSCmdBuf[num].value = value;
	if ((U8)(CSCmdBufWriteCount - CSCmdBufReadCount) < (U8)CS_COMMAND_BUF_SIZE){
		++CSCmdBufWriteCount;//overflow is ok, while CS_COMMAND_BUF_SIZE = 2^n
	}
	Cpu_SetIPL(0);		
}
    */


static void CS_cmd_to_str(char* str, S16 seq, S16 command, S16 value){
	(void)sprintf(str, "%d:%d:%d#\n", seq, command, value);	
}


static void CSSendAck(S16 seq, S16 com, S16 value){
	char str[CS_BUF_LEN + 1];
	CS_cmd_to_str(str, seq, com, value);
	ASSendData((U8*)&str[0], strlen(str));			
}
void CSParse(U8 *cp){
	S16	seq = 0, command = CS_PAUSE, value = 0;
	U8 pos = 0;
	U16 num = 0;
	CSCommand cmd;
	Cpu_SetIPL(6);
	seq = CS_atoi(cp, &pos);
	cp += pos + 1;
	command = CS_atoi(cp, &pos);
	cp += pos + 1;
	value = CS_atoi(cp, &pos);
	cmd.seq = seq;
	cmd.command = command;
	cmd.value = value;

//	num = (U16)(seq + command + value);
//	HmiShowNumber(num);
//	ASSendByte((U8)num);
//	CS_save_cmd(seq, command, value);
	CSSendAck(seq, command, value);
	CSExec(cmd);	
	Cpu_SetIPL(0);	
}

void CSIrqHandler(void){
	U8 c = ASReadEvent();
	if (CS_is_valid_char(c)){
		switch(CSpState){
			case CS_OK:{
				if (c != CS_CS){
					if (c == CS_FS){
						++CSpFSCount;
					}
					if (CSpBufPtr < CS_BUF_LEN){
						CSpBuf[CSpBufPtr] = c;
						++CSpBufPtr;
					}else{
						CSpState = CS_WAIT_CS;
					}	
				}else{
					CSpNextState = CS_OK;
					if (CSpFSCount == 2){
						CSpState = CS_BUSY;
						CSParse(&CSpBuf[0]);
						CS_flush();
						CSpState = CSpNextState;
					}else{
						CS_flush();	
					}		
				}
			}
			break;
			case CS_WAIT_CS:{
				if (c == CS_CS){
					CS_flush();
					CSpState = CS_OK;	
				}
			}
			break;
			case CS_BUSY:{
				CSpNextState = CS_WAIT_CS;	
			}
			break;
			default:
			break;
		}						 
	
	}else{
		if (c == '\n'){
			CS_flush();
			CSpState = CS_OK;		
		}else{
			CSpState = CS_WAIT_CS;	
		}
	}	
		
}

static void CSSaveParkParameter(CSCommand cmd){
	if (cmd.seq >= 1 && cmd.seq <= CS_MAX_COMMAND){
		U8 seq = cmd.seq - 1;
		switch(cmd.command){
			case CS_MODE:
				PCProcedure[seq].mode = cmd.value;	
				break;
			case CS_DIRECTION:
				PCProcedure[seq].direction = cmd.value;
				break;
			case CS_DISTANCE:
				PCProcedure[seq].distance = cmd.value;
				break;
			case CS_SPEED:
				PCProcedure[seq].speed = cmd.value;
				break;
			case CS_TRIG_MIN:
				PCProcedure[seq].trig_min = cmd.value;
				break;
			case CS_EXTENDED:
				PCProcedure[seq].extended = cmd.value;
				break;
			default:
				break;
		}
	}
}
void CSExec(CSCommand cmd){
	if (cmd.seq == 0){
		switch (cmd.command){
			case CS_PAUSE:
				CSPause();
				break;
			case CS_MOVE:
				CSMove();
				break;
			case CS_RESTART:
				CSRestart();  				
				break;
			case CS_JUMP:
				CSJump(cmd.value);
				break;
			case CS_STATUS:
				CSStatus();
				break;
			case CS_REQUIRE:
				CSRequire(cmd.value);
				break;
			case CS_SAVE:
				CSSave();
				break;
			case CS_CONTINUE:
				CSContinue();
				break;
			case CS_LOOP:
				CSLoop(cmd.value);
				break;
			default:
				break;
		}
	}else{
		CSSaveParkParameter(cmd);			
	}
}





void CSEntry(void){
	for(;;){
		switch (CSCommandState){
			case CS_WAITING:
				continue;
				break;
			case CS_RUNNING:
				PCPrepare();
				PCDrive();	
				CSCommandState = CSNextCommandState;
				break;
			default:
				break;
		}
		Cpu_Delay100US(100);
	}
}


void CSRequire(S16 seq){
	if (seq != 0){
		CSSendPCProcedure((U8)seq);	
	}
}



void CSSendPCProcedure(U8 seq_from_one){
    U8 seq = seq_from_one - 1;
	if (seq < (U8)CS_MAX_COMMAND){
		U16 i;
		S16 *ptr = (S16*)&PCProcedure[seq];
		for (i = 0; i < sizeof(ParkProcedure) / sizeof(S16); ++i){	
			U8 str[CS_BUF_LEN + 1];
			//i plus one as the initial number is 1
			CS_cmd_to_str((char*)str, seq_from_one, i + 1, *ptr++);	
			ASSendData(str, strlen((char*)str));
			Cpu_Delay100US(10);			
		}
	}
}

void CSPause(void){
	PCTimerFreeze();
	DrvFreezeMotorControl();
}

void CSMove(void){
	if (CSCommandState == CS_WAITING){
		CSCommandState = CS_RUNNING;	
	}else{
		DrvRestoreMotorControl();
		PCTimerRestore();			
	}
}

void CSRestart(void){
	PCExit();
		
}

void CSJump(S16 seq){
	PCJump(seq - 1);			
}

void CSStatus(void){
//	S16 seq, dist;
	bool status, mode;
	S16 value = 0;
	char str[CS_BUF_LEN + 1];
//	PCGetStatus(&seq, &dist);
	PCGetRunningStatus(&status, &mode);
	if (CSCommandState == CS_WAITING){
		status = FALSE;	
	}
	value += (status? 10: 0);
	value += (mode? 1:0);
	CS_cmd_to_str(str, 0, CS_STATUS_OFFSET, value);
	ASSendData((U8*)&str[0], strlen(str));							
}

void CSSave(void){
	PCSetFlagMemory(PC_FLAG_PROCEDURE_OFFSET, PC_FLAG_MAGIC);
	PCStoreProcedure();	
	Cpu_Delay100US(200);
	PCLoadProcedure();
}

void CSContinue(void){
	PCPassMileStone();
}

void CSLoop(S16 state){
	PCLoop(state);
	if (state != 0){
		CSNextCommandState = CS_RUNNING;		
	}else{
		CSNextCommandState = CS_WAITING;		
	}
}


/* END CommandSystem */
